#include "BarCalculator.h"
#include <cmath>
#include <algorithm>
#include "Utility/Algebra/MathTools.h"
#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
// #include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Result/StiffnessMatrix/IStiffnessMatrixService.h"
#include "DBManager/IService/Result/Nodal/IDISPLACEMENTService.h"
#include "DBManager/IService/Result/ElementalForce/IBARForceService.h"
#include "DBManager/IService/Result/ElementalStrain/IBARStrainService.h"
#include "DBManager/IService/Result/ElementalStress/IBARStressService.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "BarSectionParser.h"

using SG::Algebra::Matrixd;
using SG::Algebra::Vector3D;
using SG::Algebra::Point;
using SG::Algebra::PointArray;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::IdArray;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::TOLERANCE;
using namespace SG::DataStructure::FEM;
using SG::DataStructure::Post::BARForceData;
using SG::DataStructure::Post::BARStrainData;
using SG::DataStructure::Post::BARStressData;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using SG::FEMSolver::Element::BarSectionParser;
using SG::FEMSolver::Element::BarCalculator;

extern std::shared_ptr<DBServiceFactory> DB_SERVICEFACTORY;

void BarCalculator::ComputeOrienVector
(
const SG::DataStructure::FEM::NodeData&        orienNode,
const SG::DataStructure::FEM::BeamCoordOptions orienFlag,
const SG::Algebra::Point&                      pointA,
const SG::DataStructure::Common::Id            dispCoordIdA,
const SG::Algebra::Matrixd&                    TGA,
_OUT SG::Algebra::Vector3D&                    orienVector
)
{
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();

    // 使用朝向节点
    if (orienNode.m_id > 0)
    {
        // 计算朝向节点在 Basic 坐标系下的坐标
        Point orienPoint;
        Id dispCoordId;
        coordProcessor.GetGlobalCoordinate (orienNode, _OUT orienPoint, _OUT dispCoordId);

        // 在 Basic 坐标系下计算朝向节点形成的朝向矢量
        orienVector = SG::Algebra::Distance (orienPoint, pointA);
    }
    // 使用朝向矢量
    else
    {
        // 朝向矢量为位移输出坐标系（G）
        if (BeamCoordOptions::OPTION_G == orienFlag)
        {
            if (dispCoordIdA > 0)
            {
                // 朝向矢量变换到 Basic 坐标系
                orienVector = orienVector.Rotate (TGA.Transpose ());
            }
        }
    }
}

void BarCalculator::ComputeOffset
(
const std::vector<SG::DataStructure::FEM::BeamCoordOptions>& offsetFlagSet,
const SG::Algebra::Matrixd&                                  TO,
const SG::DataStructure::Common::IdArray&                    dispCoordIdSet,
const std::vector<SG::Algebra::Matrixd>&                     TGSet,
_OUT std::vector<SG::Algebra::Vector3D>&                     offsetSet,
_OUT std::vector<SG::Algebra::Vector3D>&                     offsetSetG
)
{
    for (std::size_t iNode{0}; iNode < 2; ++iNode)
    {
        if (offsetSet[iNode].IsZero ())
        {
            continue;
        }
        
        if (BeamCoordOptions::OPTION_O == offsetFlagSet[iNode + 1])
        {
            // Basic 坐标系下的偏置矢量
            offsetSet[iNode] = offsetSet[iNode].Rotate (TO.Transpose ());
            
            // 位移输出坐标系下的偏置矢量
            offsetSetG[iNode] = offsetSet[iNode];
            if (dispCoordIdSet[iNode] > 0)
            {
                offsetSetG[iNode] = offsetSetG[iNode].Rotate (TGSet[iNode]);
            }
        }
        else if (BeamCoordOptions::OPTION_G == offsetFlagSet[iNode + 1])
        {
            // 位移输出坐标系下的偏置矢量
            offsetSetG[iNode] = offsetSet[iNode];

            // Basic 坐标系下的偏置矢量
            if (dispCoordIdSet[iNode] > 0)
            {
                offsetSet[iNode] = offsetSet[iNode].Rotate (TGSet[iNode].Transpose ());
            }
        }
    }
}

void BarCalculator::AssembleElementTrans
(
const SG::Algebra::Matrixd&               T,
const SG::DataStructure::Common::IdArray& dispCoordIdSet,
const std::vector<SG::Algebra::Matrixd>&  TGSet,
const std::vector<SG::Algebra::Vector3D>& offsetSetG,
_OUT SG::Algebra::Matrixd&                TEle
)
{
    TEle = Matrixd (12, 12); // 必须初始化， 非对角块需要重置
    for (std::size_t iNode{0}; iNode < 2; ++iNode)
    {
        // 单元坐标系到位移输出坐标系变换
        Matrixd TEle2G{T};
        // 节点存在位移输出坐标系
        if (dispCoordIdSet[iNode] > 0)
        {
            TEle2G = TEle2G * TGSet[iNode].Transpose ();
        }
        const auto iNodeDof = iNode * 6;
        TEle.BlockFill (iNodeDof, iNodeDof, TEle2G);
        TEle.BlockFill (iNodeDof + 3, iNodeDof + 3, TEle2G);

        // 节点偏置变换
        if (!offsetSetG[iNode].IsZero ())
        {
            Matrixd TOffset (3, 3);
            TOffset (0, 1) = offsetSetG[iNode].m_z;
            TOffset (2, 0) = offsetSetG[iNode].m_y;
            TOffset (1, 2) = offsetSetG[iNode].m_x;
            TOffset (1, 0) = -TOffset (0, 1);
            TOffset (0, 2) = -TOffset (2, 0);
            TOffset (2, 1) = -TOffset (1, 2);
            TEle.BlockFill (iNodeDof, iNodeDof + 3, TEle2G * TOffset);
        }
    }
}

void BarCalculator::ComputeElementTrans
(
const SG::DataStructure::FEM::CBARData&              element,
const std::vector<SG::DataStructure::FEM::NodeData>& nodeSet,
const SG::DataStructure::FEM::NodeData&              orienNode,
_OUT SG::DataStructure::Common::Real&                L,
_OUT SG::Algebra::Matrixd&                           T,
_OUT SG::Algebra::Matrixd&                           TEle
)
{
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();

    // 计算 Basic 坐标系下的节点坐标，获取节点位移输出坐标系的变换矩阵
    PointArray pointSet (2);
    IdArray dispCoordIdSet (2);
    std::vector<Matrixd> TGSet (2);
    for (std::size_t iNode{0}; iNode < 2; ++iNode)
    {
        coordProcessor.GetGlobalCoordinate (nodeSet[iNode], _OUT pointSet[iNode], _OUT dispCoordIdSet[iNode]);
        if (dispCoordIdSet[iNode] > 0)
        {
            coordProcessor.GetCoordTrans (dispCoordIdSet[iNode], pointSet[iNode], _OUT TGSet[iNode]);
        }
    }

    // 计算 Basic 坐标系下的朝向矢量
    const auto& offsetFlagSet = element.GetCoordOptions ();
    Vector3D orienVector{element.m_x1, element.m_x2, element.m_x3};
    ComputeOrienVector (orienNode, offsetFlagSet[0], pointSet[0], dispCoordIdSet[0], TGSet[0], _OUT orienVector);

    // 计算单元的长度和单元坐标系的变换矩阵
    auto xVec = SG::Algebra::Distance (pointSet[1], pointSet[0]);
    xVec.Normalize (L);
    SG::Algebra::computeLocalCoordTrans (xVec, orienVector, _OUT T);

    // 如果存在节点偏置
    std::vector<Vector3D> offsetSet (2);
    offsetSet[0] = element.GetWa ();
    offsetSet[1] = element.GetWb ();
    std::vector<Vector3D> offsetSetG (2);
    if (!(offsetSet[0].IsZero () && offsetSet[1].IsZero ()))
    {
        // 节点偏置矢量变换到 Basic 坐标系和位移输出坐标系（G）
        ComputeOffset (offsetFlagSet, T, dispCoordIdSet, TGSet, _OUT offsetSet, _OUT offsetSetG);

        // 更新偏置后的节点坐标
        for (std::size_t iNode{0}; iNode < 2; ++iNode)
        {
            pointSet[iNode].Translate (offsetSet[iNode]);
        }

        // 更新单元的长度和单元坐标系的变换矩阵
        auto xVec = SG::Algebra::Distance (pointSet[1], pointSet[0]);
        xVec.Normalize (L);
        SG::Algebra::computeLocalCoordTrans (xVec, orienVector, _OUT T);
    }

    // 组装单元变换矩阵：单元坐标系到位移输出坐标系变换 + 偏置变换
    AssembleElementTrans (T, dispCoordIdSet, TGSet, offsetSetG, TEle);
}

void BarCalculator::BarCoorTrans
(
const SG::DBManager::DBServiceFactorySP dbServiceFactory,
const SG::DataStructure::FEM::CBARData& barData,
_OUT SG::Algebra::Matrixd&              TEle,
_OUT SG::Algebra::Matrixd&              T,
_OUT SG::DataStructure::Common::Real&   L
)
{
    const auto& assembly = AssemblyEigen::GetInstance ();

    // 获取单元节点数据
    std::vector<std::size_t> nodeIndexSet (2);
    std::vector<NodeData>    nodeSet (2);
    const auto& nodeService = dbServiceFactory->get<INodeService> ();
    NodeData orienNode;
    if (barData.m_go > 0)
    {
        assembly.FindNodeIndex (barData.m_go, _OUT nodeIndexSet[0]);
        nodeService->FindByIndex (nodeIndexSet[0], _OUT orienNode);
    }
    // 获取单元节点对应的索引
    assembly.FindNodeIndex (barData.m_g, _OUT nodeIndexSet);
    // 获取单元节点数据
    nodeService->FindByIndex (nodeIndexSet[0], _OUT nodeSet[0]);
    nodeService->FindByIndex (nodeIndexSet[1], _OUT nodeSet[1]);
    // 计算位移输出坐标系下单元长度、单元变换矩阵
    ComputeElementTrans (barData, nodeSet, orienNode, _OUT L, _OUT T, _OUT TEle);
}

void BarCalculator::ComputeStiffness
(
const SG::DataStructure::Common::Real&           L,
const SG::DataStructure::FEM::IsotropicMaterial& material,
const SG::DataStructure::FEM::PBARData&          property,
_OUT SG::Algebra::Matrixd&                       kl
)
{
    const Real& E{material.m_E};
    const Real& G{material.m_G};
    const Real& A{property.m_A};
    const Real& I12{property.m_I12};

    // Link
    kl = Matrixd (12, 12);
    const Real EAL{E * A / L};
    kl (0, 0) =  EAL;
    kl (0, 6) = -EAL;
    kl (6, 0) = -EAL;
    kl (6, 6) =  EAL;

    // Torsion
    const Real GJL{G * property.m_J / L};
    kl (3, 3) =  GJL;
    kl (3, 9) = -GJL;
    kl (9, 3) = -GJL;
    kl (9, 9) =  GJL;

    const Real LCube{L * L * L};
    const Real EI1{E * property.m_I1};
    const Real EI2{E * property.m_I2};
    Real R1{0.0};
    Real R2{0.0};
    // Ky > 0, 剪切修正
    if (std::fabs (I12) < TOLERANCE && property.m_K1 > TOLERANCE)
    {
        const auto GAK1 = G * A * property.m_K1;
        R1 = (12.0 * EI1 * GAK1) / (GAK1 * LCube + 12.0 * L * EI1);
    }
    else
    {
        R1 = 12.0 * EI1 / LCube;
    }

    if (std::fabs (I12) < TOLERANCE && property.m_K2 > TOLERANCE)
    {
        const auto GAK2 = G * A * property.m_K2;
        R2 = (12.0 * EI2 * GAK2) / (GAK2 * LCube + 12.0 * L * EI2);
    }
    else
    {
        R2 = 12.0 * EI2 / LCube;
    }

    // Plane 1
    kl (1, 1) = R1;          kl (1, 5) = 0.5*R1*L;            kl (1, 7) = -R1;         kl (1, 11) = 0.5*R1*L;
    kl (5, 1) = kl (1, 5);   kl (5, 5) = 0.25*R1*L*L + EI1/L; kl (5, 7) = -0.5*R1*L;   kl (5, 11) = 0.25*R1*L*L - EI1/L;
    kl (7, 1) = kl (1, 7);   kl (7, 5) = kl (5, 7);           kl (7, 7) = R1;          kl (7, 11) = -0.5*R1*L;
    kl (11, 1) = kl (1, 11); kl (11, 5) = kl (5, 11);         kl (11, 7) = kl (7, 11); kl (11, 11) = 0.25*R1*L*L + EI1/L;

    // Plane 2
    kl (2, 2) = R2;          kl (2, 4) = -0.5*R2*L;           kl (2, 8) = -R2;         kl (2, 10) = -0.5*R2*L;
    kl (4, 2) = kl (2, 4);   kl (4, 4) = 0.25*R2*L*L + EI2/L; kl (4, 8) = 0.5*R2*L;    kl (4, 10) = 0.25*R2*L*L - EI2/L;
    kl (8, 2) = kl (2, 8);   kl (8, 4) = kl (4, 8);           kl (8, 8) = R2;          kl (8, 10) = 0.5*R2*L;
    kl (10, 2) = kl (2, 10); kl (10, 4) = kl (4, 10);         kl (10, 8) = kl (8, 10); kl (10, 10) = 0.25*R2*L*L + EI2/L;

    // I12 > 0 非对称部分贡献刚度矩阵
    if (std::fabs (I12) > TOLERANCE)
    {
        const Real beta{12.0 * E * I12 / LCube};

        kl (1, 2) = beta;        kl (1, 4) = -0.5 * beta * L;     kl (1, 8) = -beta;           kl (1, 10) = -0.5 * beta * L;
        kl (2, 1) = kl (1, 2);   kl (2, 5) = 0.5 * beta * L;      kl (2, 7) = -beta;           kl (2, 11) = 0.5 * beta * L;
        kl (4, 1) = kl (1, 4);   kl (4, 5) = -beta * L * L / 3.0; kl (4, 7) = 0.5 * beta * L;  kl (4, 11) = -beta * L * L / 6.0;
        kl (5, 2) = kl (2, 5);   kl (5, 4) = kl (4, 5);           kl (5, 8) = -0.5 * beta * L; kl (5, 10) = -beta * L * L / 6.0;

        kl (7, 2) = kl (2, 7);   kl (7, 4) = kl (4, 7);           kl (7, 8) = beta;            kl (7, 10) = 0.5 * beta * L;
        kl (8, 1) = kl (1, 8);   kl (8, 5) = kl (5, 8);           kl (8, 7) = kl (7, 8);       kl (8, 11) = -0.5 * beta * L;
        kl (10, 1) = kl (1, 10); kl (10, 5) = kl (5, 10);         kl (10, 7) = kl (7, 10);     kl (10, 11) = -beta * L * L / 3.0;
        kl (11, 2) = kl (2, 11); kl (11, 4) = kl (4, 11);         kl (11, 8) = kl (8, 11);     kl (11, 10) = kl (10, 11);
    }
}

SG::Algebra::Matrixd BarCalculator::ComputeLumpedMass
(
const bool                              hasTorsion,
const SG::DataStructure::Common::Real&  L,
const SG::DataStructure::Common::Real&  rho,
const SG::DataStructure::FEM::PBARData& property
)
{
    // 集中质量矩阵
    Matrixd lumpedM (12, 12);
    const Real m{0.5 * (rho * property.m_A + property.m_NSM) * L};
    for (std::size_t i{0}; i < 3; ++i)
    {
        lumpedM (i, i) = m;
        lumpedM (i + 6, i + 6) = m;
    }
    if (hasTorsion)
    {
        lumpedM (9, 9) = lumpedM (3, 3) = 0.5 * rho * (property.m_I1 + property.m_I2) * L;
    }

    return lumpedM;
}

SG::Algebra::Matrixd BarCalculator::ComputeCoupledMass
(
const bool                              isBARMASS,
const SG::DataStructure::Common::Real&  L,
const SG::DataStructure::Common::Real&  rho,
const SG::DataStructure::FEM::PBARData& property
)
{
    Matrixd coupledM (12, 12);
    const Real m{(rho * property.m_A + property.m_NSM) * L / 420.0};
    if (isBARMASS)
    {
        // 使用一致质量矩阵
        const Real moment{rho * (property.m_I1 + property.m_I2) * L / 3.0};
        coupledM (0, 0)   = 140.0 * m;
        coupledM (1, 1)   = 156.0 * m;
        coupledM (2, 2)   = 156.0 * m;
        coupledM (3, 3)   = moment;
        coupledM (4, 4)   = 4.0 * L * L * m;
        coupledM (5, 5)   = 4.0 * L * L * m;
        coupledM (6, 6)   = 140.0 * m;
        coupledM (7, 7)   = 156.0 * m;
        coupledM (8, 8)   = 156.0 * m;
        coupledM (9, 9)   = moment;
        coupledM (10, 10) = 4.0 * L * L * m;
        coupledM (11, 11) = 4.0 * L * L * m;
        coupledM (0, 6)   = 70.0 * m;
        coupledM (1, 5)   = 22.0 * L * m;
        coupledM (1, 7)   = 54.0 * m;
        coupledM (1, 11)  = -13.0 * L * m;
        coupledM (2, 4)   = -22.0 * L * m;
        coupledM (2, 8)   = 54.0 * m;
        coupledM (2, 10)  = 13.0 * L * m;
        coupledM (3, 9)   = 0.5 * moment;
        coupledM (4, 8)   = -13.0 * L * m;
        coupledM (4, 10)  = -3.0 * L * L * m;
        coupledM (5, 7)   = 13.0 * L * m;
        coupledM (5, 11)  = -3.0 * L * L * m;
        coupledM (7, 11)  = -22.0 * L * m;
        coupledM (8, 10)  = 22.0 * L * m;
    }
    else
    {
        // 使用耦合质量矩阵（轴向平动惯性质量采用一致质量和集中质量的平均，不考虑轴向转动惯性质量）
        coupledM (0, 0)   = 175.0 * m;
        coupledM (1, 1)   = 156.0 * m;
        coupledM (2, 2)   = 156.0 * m;
        coupledM (4, 4)   = 4.0 * L * L * m;
        coupledM (5, 5)   = 4.0 * L * L * m;
        coupledM (6, 6)   = 175.0 * m;
        coupledM (7, 7)   = 156.0 * m;
        coupledM (8, 8)   = 156.0 * m;
        coupledM (10, 10) = 4.0 * L * L * m;
        coupledM (11, 11) = 4.0 * L * L * m;
        coupledM (0, 6)   = 35.0 * m;
        coupledM (1, 5)   = 22.0 * L * m;
        coupledM (1, 7)   = 54.0 * m;
        coupledM (1, 11)  = -13.0 * L * m;
        coupledM (2, 4)   = -22.0 * L * m;
        coupledM (2, 8)   = 54.0 * m;
        coupledM (2, 10)  =  13.0 * L * m;
        coupledM (4, 8)   = -13.0 * L * m;
        coupledM (4, 10)  = -3.0 * L * L * m;
        coupledM (5, 7)   = 13.0 * L * m;
        coupledM (5, 11)  = -3.0 * L * L * m;
        coupledM (7, 11)  = -22.0 * L * m;
        coupledM (8, 10)  = 22.0 * L * m;
    }

    // symmetry
    for (std::size_t i{0}; i < 12; ++i)
    {
        for (std::size_t j{0}; j < i; ++j)
        {
            coupledM (i, j) = coupledM (j, i);
        }
    }

    return coupledM;
}

void BarCalculator::ReleaseMassDof (const std::vector<std::size_t>& freeDofSet, const SG::Algebra::Matrixd& kl, _OUT SG::Algebra::Matrixd& ml)
{
    for (const auto& f : freeDofSet)
    {
        const Real klff{kl (f, f)};
        if (std::fabs (klff) < TOLERANCE)
        {
            for (std::size_t i{0}; i < 12; ++i)
            {
                ml (f, i) = 0.0;
                ml (i, f) = 0.0;
            }
            continue;
        }

        const Matrixd mlTmp{ml};
        for (std::size_t i{0}; i < 12; ++i)
        {
            const Real ratiofi{kl (f, i) / klff};
            for (std::size_t j{0}; j < 12; ++j)
            {
                const Real ratiojf{kl (j, f) / klff};
                ml (i, j) = mlTmp (i, j) - ratiojf * mlTmp (f, i) - ratiofi * mlTmp (j, f) + ratiojf * ratiofi * mlTmp (f, f);
            }
        }
    }
}

void BarCalculator::ReleaseStiffnessDof (const std::vector<std::size_t>& freeDofSet, _OUT SG::Algebra::Matrixd& kl)
{
    for (const auto& f : freeDofSet)
    {
        const Real klff{kl (f, f)};
        if (std::fabs (klff) < TOLERANCE)
        {
            for (std::size_t i{0}; i < 12; ++i)
            {
                kl (f, i) = 0.0;
                kl (i, f) = 0.0;
            }
            continue;
        }

        const Matrixd klTmp{kl};
        for (std::size_t i{0}; i < 12; ++i)
        {
            const Real ratioif{klTmp (i, f) / klff};
            for (std::size_t j{0}; j < 12; ++j)
            {
                kl (i, j) = klTmp (i, j) - klTmp (f, j) * ratioif;
            }
            kl (f, i) = 0.0;
            kl (i, f) = 0.0;
        }
    }
}

void BarCalculator::ReleaseLoadDof
(
const SG::DataStructure::FEM::DofSet& dofsPaRelease,
const SG::DataStructure::FEM::DofSet& dofsPbRelease,
SG::Algebra::Matrixd&                 stiffMat,
_OUT SG::Algebra::Matrixd&            loadMat
)
{
    // 若无自由度释放直接跳出
    if (dofsPaRelease.empty () && dofsPbRelease.empty ())
    {
        return;
    }

    // 合并A/B端释放自由度为全局索引（A端直接转std::size_t，B端转std::size_t后+6）
    std::vector<std::size_t> releaseIndices;
    releaseIndices.reserve (dofsPaRelease.size () + dofsPbRelease.size ());

    // 处理A端：方向值直接转为全局索引（0-5）
    for (DOFDirections dir : dofsPaRelease)
    {
        if (DOFDirections::DOF_Unknow == dir) continue;
        std::size_t idx = static_cast<std::size_t> (dir);
        releaseIndices.push_back (idx);
    }

    // 处理B端：方向值+6转为全局索引（6-11）
    for (DOFDirections dir : dofsPbRelease)
    {
        if (DOFDirections::DOF_Unknow == dir) continue;
        std::size_t idx = static_cast<std::size_t> (dir) + 6;  // B端偏移+6
        if (6 <= idx && idx < 12) {  // 校验B端有效范围
            releaseIndices.push_back (idx);
        }
    }

    // 处理自由度释放（注意：荷载释放要与刚度矩阵释放同步进行！）
    for (std::size_t globalIdx : releaseIndices)
    {
        Real kii = stiffMat (globalIdx, globalIdx);
        const Matrixd stiffMatTmp{stiffMat};
        if (std::fabs (kii) > TOLERANCE)         // 校验刚度对角元（避免除以零）
        {
            for (std::size_t i{0}; i < 12; ++i)
            {
                if (i == globalIdx) continue;

                // 处理荷载
                loadMat (i, 0) -= (stiffMatTmp (i, globalIdx) / kii) * loadMat (globalIdx, 0);
                
                //处理刚度矩阵
                const Real ratioif{stiffMatTmp (i, globalIdx) / kii};
                for (std::size_t j{0}; j < 12; ++j)
                {
                    stiffMat (i, j) = stiffMatTmp (i, j) - stiffMatTmp (globalIdx, j) * ratioif;
                }
            }
        }

        for (std::size_t i{0}; i < 12; ++i)
        {
            stiffMat (globalIdx, i) = 0.0;
            stiffMat (i, globalIdx) = 0.0;
        }

        loadMat (globalIdx, 0) = 0.0;
    }
}

void BarCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
}

void BarCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();

    CBARData element;
    std::vector<std::size_t> nodeIndexSet (2);
    std::vector<int>         dofGlobalIndexSet (12);
    std::vector<NodeData>    nodeSet (2);
    static const DofSet dofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                               DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz};
    Real L;
    Matrixd T (3, 3);
    Matrixd TEle (12, 12);
    Matrixd kg (12, 12);
    Matrixd massMatrix (12, 12);

    // 解析 CBAR 单元的质量全局控制选项
    const auto needCoupleMass = assembly.NeedCoupleMass ();
    bool is414{false};
    bool isBARMASS{false};
    const auto result = dbServiceFactory->get<IParameterService> ()->GetExecutiveSystemParameter ();
    for (std::size_t i{0}; i < result.size (); ++i)
    {
        SG::Utility::SGParameter param = result[i];
        if (param["KEY"].GetString () == "SYSTEM" && param["I"].GetString () == "414")
        {
            is414 = std::stoi (param["VALUE"].GetString ()) > 0;
        }
        else if (param["KEY"].GetString () == "BARMASS")
        {
            isBARMASS = std::stoi (param["VALUE"].GetString ()) > 0;
        }
    }
    // 仅当 isBARMASS 为 ture 且 is414 为 false 时，集中质量矩阵应包含转动项
    bool hasTorsion{false};
    if(!is414 && isBARMASS)
    {
        hasTorsion = true;
    }

    // 单元属性
    BarSectionParser secParser;
    PBARData property;
    IsotropicMaterial material;

    const auto& nodeService = dbServiceFactory->get<INodeService> ();
    const auto& eleService  = dbServiceFactory->get<IElementService> ();
    const auto  eleSize     = eleService->GetElementSize (m_type);
    // 存储单元坐标系下单元刚度矩阵
    std::vector<Matrixd> localStiffnessSet (eleSize);

    for (std::size_t iEle{0}; iEle < eleSize; ++iEle)
    {
        eleService->FindByIndex (iEle, _OUT element);

        // 使用朝向节点时，获取朝向节点对应的索引，以及朝向节点数据
        NodeData orienNode;
        if (element.m_go > 0)
        {
            assembly.FindNodeIndex (element.m_go, _OUT nodeIndexSet[0]);
            nodeService->FindByIndex (nodeIndexSet[0], _OUT orienNode);
        }

        // 获取单元节点对应的索引
        assembly.FindNodeIndex (element.m_g, _OUT nodeIndexSet);

        // 获取单元自由度对应的全局自由度索引
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);

        // 获取单元节点数据
        nodeService->FindByIndex (nodeIndexSet[0], _OUT nodeSet[0]);
        nodeService->FindByIndex (nodeIndexSet[1], _OUT nodeSet[1]);

        // 计算位移输出坐标系下单元长度、单元坐标系变换矩阵、单元变换矩阵
        ComputeElementTrans (element, nodeSet, orienNode, _OUT L, _OUT T, _OUT TEle);

        // 查询单元属性和材料属性
        secParser.GetSectionProperty (dbServiceFactory, element.m_pId, _OUT property);
        secParser.GetMatrial (property.m_mId, _OUT material);

        // 计算单元坐标系下单元刚度矩阵
        ComputeStiffness (L, material, property, _OUT kg);

        // 计算单元坐标系下单元质量矩阵
        if (needCoupleMass)
        {
            massMatrix = ComputeCoupledMass (isBARMASS, L, material.m_rho, property);
        }
        else
        {
            massMatrix = ComputeLumpedMass (hasTorsion, L, material.m_rho, property);
        }

        // 对单元坐标系下单元耦合质量矩阵和刚度矩阵进行自由度释放（Pin）
        std::vector<std::size_t> freeDofSet;
        for (const auto& dof : element.GetDofSetOfPaRelease ())
        {
            freeDofSet.push_back (static_cast<std::size_t> (dof));
        }
        for (const auto& dof: element.GetDofSetOfPbRelease ())
        {
            freeDofSet.push_back (static_cast<std::size_t> (dof) + 6);
        }
        if (!freeDofSet.empty ())
        {
            // 由于单元质量矩阵自由度释放需要用到未自由度释放的单元刚度矩阵，需要先进行单元质量矩阵自由度释放
            if(needCoupleMass)
            {
                // 集中质量阵不需要自由度释放
                ReleaseMassDof (freeDofSet, kg, _OUT massMatrix);
            }
            ReleaseStiffnessDof (freeDofSet, _OUT kg);
        }

        // 存储单元坐标系下自由度释放后的单元刚度矩阵（供计算单元内力使用）
        localStiffnessSet[iEle] = kg;

        // 单元刚度矩阵变换到位移输出坐标系，并累加到总体刚度矩阵
        kg = TEle.Transpose () * kg * TEle;
        assembly.AccumulateStiffness (dofGlobalIndexSet, kg);

        // 单元质量矩阵变换到位移输出坐标系，并累加到总体质量矩阵
        // 注意，集中质量矩阵为各项同性矩阵，不需要进行坐标变换；但是，含转动项的集中质量矩阵需要进行坐标变换，有偏置的需要进行偏置变换
        if(!is414)
        {
            massMatrix = TEle.Transpose () * massMatrix * TEle;
        }
        assembly.AccumulateCoupledMass (dofGlobalIndexSet, massMatrix);
    }

    dbServiceFactory->get<IStiffnessMatrixService> ()->Add (m_type, localStiffnessSet);
}

void BarCalculator::ComputeElementInternalForce
(
const SG::Algebra::Matrixd& kl,
const SG::Algebra::Matrixd& TEle,
const SG::Algebra::Matrixd& elementDisplacement,
_OUT SG::Algebra::Matrixd&  elementInternalForce
)
{
    // 单元坐标系单元内力
    elementInternalForce = kl * TEle * elementDisplacement;

    // ToDo：温度、初应变
}

void BarCalculator::ComputeElementStress
(
const SG::Algebra::Matrixd&             elementInternalForce,
const SG::DataStructure::FEM::PBARData& property,
_OUT SG::Algebra::Matrixd&              elementStress
)
{
    elementStress (0, 0)  = -elementInternalForce (0, 0) / property.m_A;
    
    const auto m1a = -elementInternalForce (5, 0);
    const auto m2a =  elementInternalForce (4, 0);
    const auto m1b =  elementInternalForce (11, 0);
    const auto m2b = -elementInternalForce (10, 0);
    
    const auto I1  = property.m_I1;
    const auto I2  = property.m_I2;
    const auto I12 = property.m_I12;

    const auto tmp = I1 * I2 - I12 * I12;
    const auto k1a = (m2a * I12  - m1a * I2) / tmp;
    const auto k2a = (m1a * I12  - m2a * I1) / tmp;
    elementStress (1, 0) = k1a * property.m_C1 + k2a * property.m_C2;
    elementStress (2, 0) = k1a * property.m_D1 + k2a * property.m_D2;
    elementStress (3, 0) = k1a * property.m_E1 + k2a * property.m_E2;
    elementStress (4, 0) = k1a * property.m_F1 + k2a * property.m_F2;

    const auto k1b = (m2b * I12  - m1b * I2) / tmp;
    const auto k2b = (m1b * I12  - m2b * I1) / tmp;
    elementStress (5, 0) = k1b * property.m_C1 + k2b * property.m_C2;
    elementStress (6, 0) = k1b * property.m_D1 + k2b * property.m_D2;
    elementStress (7, 0) = k1b * property.m_E1 + k2b * property.m_E2;
    elementStress (8, 0) = k1b * property.m_F1 + k2b * property.m_F2;
}

void BarCalculator::ComputeElementStrain
(
const SG::Algebra::Matrixd&            elementStress,
const SG::DataStructure::Common::Real& E,
_OUT SG::Algebra::Matrixd&             elementStrain
)
{
    elementStrain = elementStress * (1.0 / E);
}

// 后处理数据
void BarCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int subcaseId, SG::DataStructure::Common::Real time, const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
    // 工况信息
    // Subcase subcase;
    // 获取后处理输出变量
    // dbServiceFactory->get<ISubcaseService> ()->FindById (subcaseId, _OUT subcase);
    // OutputSetting& outPutSetList = subcase.m_out;
    // 计算输出内力
    // 需修改输出控制，暂时以 true 取代
    // if (outPutSetList.m_force) {
    if (true)
    {
        const auto& assembly = AssemblyEigen::GetInstance ();

        CBARData element;
        std::vector<std::size_t> nodeIndexSet (2);
        std::vector<int>         dofGlobalIndexSet (12);
        std::vector<NodeData>    nodeSet (2);
        static const DofSet dofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                                   DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz};
        Real L;

        BarSectionParser secParser;
        PBARData property;
        IsotropicMaterial material;

        Matrixd T (3, 3);
        Matrixd TEle (12, 12);
        Matrixd eleDisplacement (12, 1);
        Matrixd eleInternalForce (12, 1);
        Matrixd eleStrain (9, 1);
        Matrixd eleStress (9, 1);

        // 获取单元坐标系下单元刚度矩阵列表
        std::vector<Matrixd> localStiffnessSet;
        dbServiceFactory->get<IStiffnessMatrixService> ()->Get (m_type, _OUT localStiffnessSet);

        const auto& nodeService = dbServiceFactory->get<INodeService> ();
        const auto& eleService  = dbServiceFactory->get<IElementService> ();
        const auto  eleSize     = eleService->GetElementSize (m_type);
        BARForceData eleForceData;
        std::vector<BARForceData> eleForceDataSet (eleSize);
        BARStrainData eleStrainData;
        std::vector<BARStrainData> eleStrainDataSet (eleSize);
        BARStressData eleStressData;
        std::vector<BARStressData> eleStressDataSet (eleSize);

        // 对 Bar 类型单元进行遍历
        for (std::size_t iEle{0}; iEle < eleSize; ++iEle)
        {
            eleService->FindByIndex (iEle, _OUT element);

            // 使用朝向节点时，获取朝向节点对应的索引，以及朝向节点数据
            NodeData orienNode;
            if (element.m_go > 0)
            {
                assembly.FindNodeIndex (element.m_go, _OUT nodeIndexSet[0]);
                nodeService->FindByIndex (nodeIndexSet[0], _OUT orienNode);
            }

            // 获取单元节点对应的索引
            assembly.FindNodeIndex (element.m_g, _OUT nodeIndexSet);

            // 获取单元节点数据
            nodeService->FindByIndex (nodeIndexSet[0], _OUT nodeSet[0]);
            nodeService->FindByIndex (nodeIndexSet[1], _OUT nodeSet[1]);

            // 计算位移输出坐标系下单元长度、单元坐标系变换矩阵、单元变换矩阵
            ComputeElementTrans (element, nodeSet, orienNode, _OUT L, _OUT T, _OUT TEle);

            // 获取单元自由度对应的全局自由度索引
            assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);

            // 从总体位移向量中提取单元位移
            for (int iDof{0}; iDof < 12; ++iDof)
            {
                eleDisplacement (iDof, 0) = globalDisplacement[dofGlobalIndexSet[iDof]];
            }

            // 计算单元内力
            ComputeElementInternalForce (localStiffnessSet[iEle], TEle, eleDisplacement, _OUT eleInternalForce);

            // 转换为存储的单元内力数据结构
            eleForceData.m_eId  =  element.m_id;
            eleForceData.m_AF   = -eleInternalForce (0, 0);
            eleForceData.m_TS1  = -eleInternalForce (1, 0);
            eleForceData.m_TS2  = -eleInternalForce (2, 0);
            eleForceData.m_TRQ  = -eleInternalForce (3, 0);
            eleForceData.m_BM1A = -eleInternalForce (5, 0);
            eleForceData.m_BM2A =  eleInternalForce (4, 0);
            eleForceData.m_BM1B =  eleInternalForce (11, 0);
            eleForceData.m_BM2B = -eleInternalForce (10, 0);
            eleForceDataSet[iEle] = eleForceData;

            // 查询单元属性，计算梁单元轴向正应力及截面应力恢复点处由弯曲产生的轴向正应力
            secParser.GetSectionProperty (dbServiceFactory, element.m_pId, _OUT property);
            ComputeElementStress (eleInternalForce, property, _OUT eleStress);

            // 转换为存储的单元应力数据结构
            eleStressData.m_eId = element.m_id;
            eleStressData.m_X1A = eleStress (1, 0);
            eleStressData.m_X2A = eleStress (2, 0);
            eleStressData.m_X3A = eleStress (3, 0);
            eleStressData.m_X4A = eleStress (4, 0);
            eleStressData.m_AX  = eleStress (0, 0);
            const auto minmaxStressA = std::minmax_element (eleStress[1], eleStress[5]);
            eleStressData.m_MAXA = eleStress (0, 0) + *minmaxStressA.second;
            eleStressData.m_MINA = eleStress (0, 0) + *minmaxStressA.first;

            eleStressData.m_X1B = eleStress (5, 0);
            eleStressData.m_X2B = eleStress (6, 0);
            eleStressData.m_X3B = eleStress (7, 0);
            eleStressData.m_X4B = eleStress (8, 0);
            const auto minmaxStressB = std::minmax_element (eleStress[5], eleStress[5] + 4);
            eleStressData.m_MAXB = eleStress (0, 0) + *minmaxStressB.second;
            eleStressData.m_MINB = eleStress (0, 0) + *minmaxStressB.first;

            eleStressData.m_MST = 0.0;  // ToDo: Margin of Safety in Tension
            eleStressData.m_MSC = 0.0;  // ToDo: Margin of Safety in Compression
            eleStressDataSet[iEle] = eleStressData;

            // 查询材料属性，计算单元应变
            secParser.GetMatrial (property.m_mId, _OUT material);
            ComputeElementStrain (eleStress, material.m_E, _OUT eleStrain);

            // 转换为存储的单元应变数据结构
            eleStrainData.m_eId = element.m_id;
            eleStrainData.m_X1A = eleStrain (1, 0);
            eleStrainData.m_X2A = eleStrain (2, 0);
            eleStrainData.m_X3A = eleStrain (3, 0);
            eleStrainData.m_X4A = eleStrain (4, 0);
            eleStrainData.m_AX  = eleStrain (0, 0);
            const auto minmaxStrainA = std::minmax_element (eleStrain[1], eleStrain[5]);
            eleStrainData.m_MAXA = eleStrain (0, 0) + *minmaxStrainA.second;
            eleStrainData.m_MINA = eleStrain (0, 0) + *minmaxStrainA.first;

            eleStrainData.m_X1B = eleStrain (5, 0);
            eleStrainData.m_X2B = eleStrain (6, 0);
            eleStrainData.m_X3B = eleStrain (7, 0);
            eleStrainData.m_X4B = eleStrain (8, 0);
            const auto minmaxStrainB = std::minmax_element (eleStrain[5], eleStrain[5] + 4);
            eleStrainData.m_MAXB = eleStrain (0, 0) + *minmaxStrainB.second;
            eleStrainData.m_MINB = eleStrain (0, 0) + *minmaxStrainB.first;

            eleStrainData.m_MST = 0.0;  // ToDo: Margin of Safety in Tension
            eleStrainData.m_MSC = 0.0;  // ToDo: Margin of Safety in Compression
            eleStrainDataSet[iEle] = eleStrainData;
        }

        dbServiceFactory->get<IBARForceService> ()->AddBySubcaseId (static_cast<Id> (subcaseId), eleForceDataSet);
        dbServiceFactory->get<IBARStrainService> ()->AddBySubcaseId (static_cast<Id> (subcaseId), eleStrainDataSet);
        dbServiceFactory->get<IBARStressService> ()->AddBySubcaseId (static_cast<Id> (subcaseId), eleStressDataSet);
    }
}

// 计算全局坐标系下单元应力刚度矩阵
void BarCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{
    auto& assembly = AssemblyEigen::GetInstance ();

    CBARData element;
    std::vector<std::size_t> nodeIndexSet (2);
    std::vector<int>         dofGlobalIndexSet (12);
    std::vector<NodeData>    nodeSet (2);
    static const DofSet dofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                               DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz};
    Real L;
    Matrixd T (3, 3);
    Matrixd TEle (12, 12);
    // 单元应力刚度矩阵
    Matrixd kSigma (12, 12);

    BarSectionParser secParser;
    PBARData property;

    // 读取 staticSubId 下的单元载荷
    std::vector<BARForceData> eleForceDataSet;
    dbServiceFactory->get<IBARForceService> ()->Find ({SOLType::SOL_SESTATIC, static_cast<Id> (staticSubId)}, _OUT eleForceDataSet);

    // 统计 Bar 单元数量
    const auto& nodeService = dbServiceFactory->get<INodeService> ();
    const auto& eleService  = dbServiceFactory->get<IElementService> ();
    const auto  eleSize     = eleService->GetElementSize (m_type);

    // 对 Bar 类型单元进行遍历
    for (std::size_t iELe{0}; iELe < eleSize; ++iELe)
    {
        // 单元局部坐标系下内力
        const auto& eleForceData = eleForceDataSet[iELe];

        eleService->FindByIndex (iELe, _OUT element);

        // 使用朝向节点时，获取朝向节点对应的索引，以及朝向节点数据
        NodeData orienNode;
        if (element.m_go > 0)
        {
            assembly.FindNodeIndex (element.m_go, _OUT nodeIndexSet[0]);
            nodeService->FindByIndex (nodeIndexSet[0], _OUT orienNode);
        }

        // 获取单元节点对应的索引
        assembly.FindNodeIndex (element.m_g, _OUT nodeIndexSet);

        // 获取单元自由度对应的全局自由度索引
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);

        // 由于应力刚度局部坐标系与刚度矩阵局部坐标系相反
        // 在此构造应力刚度局部坐标系与全局坐标系的转换矩阵
        nodeService->FindByIndex (nodeIndexSet[0], _OUT nodeSet[1]);
        nodeService->FindByIndex (nodeIndexSet[1], _OUT nodeSet[0]);

        // 计算位移输出坐标系下单元长度、单元坐标系变换矩阵、单元变换矩阵
        ComputeElementTrans (element, nodeSet, orienNode, _OUT L, _OUT T, _OUT TEle);

        // 查询单元属性
        secParser.GetSectionProperty (dbServiceFactory, element.m_pId, _OUT property);

        // 截面参数
        const Real A{property.m_A};
        const Real I1{property.m_I1};
        const Real I2{property.m_I2};

        // 将载荷转换至计算应力刚度矩阵的局部坐标系中
        const Real Fx{eleForceData.m_AF};
        const Real VY{-eleForceData.m_TS1};
        const Real VZ{eleForceData.m_TS2};
        const Real MAY{eleForceData.m_BM2A};
        const Real MAZ{eleForceData.m_BM1A};
        const Real MBY{-eleForceData.m_BM2B};
        const Real MBZ{-eleForceData.m_BM1B};

        // 构造局部坐标系下单元应力刚度矩阵
        // 梁应力刚度矩阵局部坐标系定义为 B 点指向 A 点为 x 轴
        // A 点指向定位节点的向量用于定义 y 轴
        const Real KsTerm1{1.2 * Fx / L};
        const Real KsTerm2{-MAY / L};
        const Real KsTerm3{Fx / 10};
        const Real KsTerm4{-MBY / L};
        const Real KsTerm5{-MAZ / L};
        const Real KsTerm6{-MBZ / L};
        const Real KsTerm7{(I1 + I2) * Fx / (L * A)};
        const Real KsTerm8{L * VY / 6};
        const Real KsTerm9{L * VZ / 6};
        const Real KsTerm10{2 * L * Fx / 15};
        const Real KsTerm11{L * Fx / 30};

        kSigma = Matrixd (12, 12);
        kSigma (1, 1)   = KsTerm1;
        kSigma (2, 2)   = KsTerm1;
        kSigma (3, 3)   = KsTerm7;
        kSigma (4, 4)   = KsTerm10;
        kSigma (5, 5)   = KsTerm10;
        kSigma (7, 7)   = KsTerm1;
        kSigma (8, 8)   = KsTerm1;
        kSigma (9, 9)   = KsTerm7;
        kSigma (10, 10) = KsTerm10;
        kSigma (11, 11) = KsTerm10;
        kSigma (1, 3)   = KsTerm2;
        kSigma (3, 1)   = KsTerm2;
        kSigma (1, 5)   = -KsTerm3;
        kSigma (5, 1)   = -KsTerm3;
        kSigma (1, 7)   = -KsTerm1;
        kSigma (7, 1)   = -KsTerm1;
        kSigma (1, 9)   = KsTerm4;
        kSigma (9, 1)   = KsTerm4;
        kSigma (1, 11)  = -KsTerm3;
        kSigma (11, 1)  = -KsTerm3;
        kSigma (2, 3)   = KsTerm5;
        kSigma (3, 2)   = KsTerm5;
        kSigma (2, 4)   = KsTerm3;
        kSigma (4, 2)   = KsTerm3;
        kSigma (2, 8)   = -KsTerm1;
        kSigma (8, 2)   = -KsTerm1;
        kSigma (2, 9)   = KsTerm6;
        kSigma (9, 2)   = KsTerm6;
        kSigma (2, 10)  = KsTerm3;
        kSigma (10, 2)  = KsTerm3;
        kSigma (3, 4)   = -KsTerm8;
        kSigma (4, 3)   = -KsTerm8;
        kSigma (3, 5)   = -KsTerm9;
        kSigma (5, 3)   = -KsTerm9;
        kSigma (3, 7)   = -KsTerm2;
        kSigma (7, 3)   = -KsTerm2;
        kSigma (3, 8)   = -KsTerm5;
        kSigma (8, 3)   = -KsTerm5;
        kSigma (3, 9)   = -KsTerm7;
        kSigma (9, 3)   = -KsTerm7;
        kSigma (3, 10)  = KsTerm8;
        kSigma (10, 3)  = KsTerm8;
        kSigma (3, 11)  = KsTerm9;
        kSigma (11, 3)  = KsTerm9;
        kSigma (4, 8)   = -KsTerm3;
        kSigma (8, 4)   = -KsTerm3;
        kSigma (4, 9)   = KsTerm8;
        kSigma (9, 4)   = KsTerm8;
        kSigma (4, 10)  = -KsTerm11;
        kSigma (10, 4)  = -KsTerm11;
        kSigma (5, 7)   = KsTerm3;
        kSigma (7, 5)   = KsTerm3;
        kSigma (5, 9)   = KsTerm9;
        kSigma (9, 5)   = KsTerm9;
        kSigma (5, 11)  = -KsTerm11;
        kSigma (11, 5)  = -KsTerm11;
        kSigma (7, 9)   = -KsTerm4;
        kSigma (9, 7)   = -KsTerm4;
        kSigma (7, 11)  = KsTerm3;
        kSigma (11, 7)  = KsTerm3;
        kSigma (8, 9)   = -KsTerm6;
        kSigma (9, 8)   = -KsTerm6;
        kSigma (8, 10)  = -KsTerm3;
        kSigma (10, 8)  = -KsTerm3;
        kSigma (9, 10)  = -KsTerm8;
        kSigma (10, 9)  = -KsTerm8;
        kSigma (9, 11)  = -KsTerm9;
        kSigma (11, 9)  = -KsTerm9;

        // 将局部坐标系应力刚度矩阵转换至单元坐标系
        kSigma = TEle.Transpose () * kSigma * TEle;

        assembly.AccumulateStressStiffness (dofGlobalIndexSet, kSigma);
    }
}